/*
 * communication.cpp
 *
 *  Created on: 05.12.2013
 *      Author: Franz Steinmetz
 */

#include "communication.h"

#include <StandardCplusplus.h>
#include <vector>
#include <stdint.h>
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"


#define UNDEFINED ((uint8_t) 0)
#define MASTER ((uint8_t) 1)
#define SLAVE ((uint8_t) 2)

using namespace std;


volatile bool sending = false;
volatile bool received_msg = false;
volatile bool interrupt = false;

// Set up nRF24L01 radio on SPI bus plus pins 5 = CE & 4 = CSN
RF24 radio(5, 4);

//
// Topology
//

const uint8_t MAX_SLAVES = 6;

const uint64_t PIPE_COMMON_PART = 0xC2C2C2C200LL;
const uint64_t PIPES[5] = { 0x0000000001LL, 0x0000000002LL, 0x0000000003LL, 0x0000000004LL, 0x0000000005LL };



// The role of the current running sketch
uint8_t role = UNDEFINED;
unsigned char buf[MSG_LENGTH];

vector<uint64_t> slaves;
uint64_t com_pipe = DEFAULT_PIPE;
message msg = message(0, ERROR);
bool debug = false;

void com_check_interrupt();
void com_set_pipe(uint64_t);
bool com_check_for_role(uint8_t role);

void com_common_init() {
	// Make sure hardware has time to configure
	delayMicroseconds(100);

	radio.begin();
	radio.setRetries(1,15); // 15 retries, minimum time in between them
	radio.setPayloadSize(MSG_LENGTH);
	radio.setDataRate(RF24_250KBPS);
	radio.setChannel(0);
	radio.setAutoAck(true);

	if(debug)
		radio.printDetails();
}

void com_init_master() {
	if(!com_check_for_role(UNDEFINED)) // Do not allow to initialize twice
		return;

	role = MASTER;

	com_common_init();

	com_pipe = DEFAULT_PIPE;
	slaves.reserve(MAX_SLAVES);

	radio.openReadingPipe(0, DEFAULT_PIPE);
	radio.startListening();

	Serial.println("Master initialized");
	//attachInterrupt(0, com_check_interrupt, FALLING); // Interrupt on D2
}

void com_init_slave() {
	if(!com_check_for_role(UNDEFINED)) // Do not allow to initialize twice
		return;

	role = SLAVE;

	com_common_init();
	com_set_pipe(DEFAULT_PIPE);
	com_pipe = DEFAULT_PIPE;

	Serial.println("Slave initialized");

	// attachInterrupt(1, com_check_interrupt, FALLING); // no interrupt on the slave
}

uint64_t com_create_pipe_adr(uint64_t lsb) {
	return PIPE_COMMON_PART & lsb;
}

bool com_wait_and_read_message(uint8_t pipe, uint32_t timeout_millis) {
	if(received_msg || timeout_millis == 0) {
		if(debug)
			Serial.println("Either received_msg flag is set or timeout is zero");
		if(!received_msg)
			radio.startListening();
		if(radio.available(&pipe)) {
			bool success =  radio.read(buf, MSG_LENGTH);
			if(!radio.available())
				received_msg = false;
			return success;
		} else {
			if(debug)
				Serial.println("Error: Message available, but on wrong pipe, or timeout was set to zero");
			return false;
		}
	}

	radio.startListening();
	for(uint32_t timeout = 0; timeout < timeout_millis; timeout += 10) {
		delay(10);
		if((pipe == ANY_SLAVE && radio.available()) || (pipe != ANY_SLAVE && radio.available(&pipe))) {
			received_msg = false;
			return radio.read(buf, MSG_LENGTH);
		}
	}


	if(debug) {
		Serial.print("Error: Timeout while waiting for message (ms): ");
		Serial.println(timeout_millis, DEC);
	}
	return false;
}

message com_read_msg(uint8_t pipe, uint32_t timeout_millis) {
	bool success = com_wait_and_read_message(pipe, timeout_millis);
	if(!success)
		return message(0, ERROR);

	return message(buf, MSG_LENGTH);
}

message com_read_msg_from_slave(uint8_t slave, uint32_t timeout_millis) {
	if(!com_check_for_role(MASTER)) // Only allowed when being master
		return message(0, ERROR);

	if(slave == 0) {
		radio.openReadingPipe(0, DEFAULT_PIPE);
	} else if (slave <= slaves.size()){
		radio.openReadingPipe(slave, slaves[slave-1]);
	} else {
		if(debug)
			Serial.println("Error: Invalid slave number for reading");
		return message(0, ERROR);
	}

	return com_read_msg(slave, timeout_millis);
}

message com_read_msg_from_master(uint32_t timeout_millis) {
	if(!com_check_for_role(SLAVE)) // Only allowed when being slave
		return message(0, ERROR);

	radio.openReadingPipe(1, com_pipe);

	return com_read_msg(1, timeout_millis);
}

bool com_send_msg_to_master(message msg) {
	if(!com_check_for_role(SLAVE)) // Only allowed when being slave
		return false;

	radio.stopListening();
	radio.openWritingPipe(com_pipe);
	sending = true;
	bool success = false;
	for(uint8_t c = 0; c < 10; c++) {
		success = radio.write(msg.toCharArray(), MSG_LENGTH);
		if(success)
			break;
	}
	sending = false;
	if(!success && debug)
		Serial.println("Error while sending msg to master");
	radio.startListening();
	return success;
}

bool com_send_msg_to_slave(message msg, uint8_t slave) {
	if(!com_check_for_role(MASTER)) // Only allowed when being master
		return false;

	if(slave == 0)
		radio.openWritingPipe(com_pipe);
	else if(slave <= slaves.size())
		radio.openWritingPipe(slaves.at(slave-1));
	else {
		if(debug)
			Serial.println("Error: Invalid slave number for writing");
		return false;
	}

	radio.stopListening();
	sending = true;
	bool success = false;
	for(uint8_t c = 0; c < 10; c++) {
		success = radio.write(msg.toCharArray(), MSG_LENGTH);
		if(success)
			break;
	}
	sending = false;
	if(!success && debug)
		Serial.println("Error while sending msg to slave");
	radio.openReadingPipe(0, com_pipe); // zero pipe has to be reopened after writing
	radio.startListening();
	return success;
}

bool com_pair_with_slave() {
	if(!com_check_for_role(MASTER)) // Only allowed when being master
		return false;

	// Check whether a slave really wants to pair
	message msg = com_read_msg_from_slave(0, 5000);
	if(msg.getComType() != PAIRING) {
		if(debug)
			Serial.println("Error: Did not receive pairing request");
		return false;
	}

	if(debug)
		Serial.println("Received pairing request");

	// Check whether we already reached the limit of slaves
	if(slaves.size() == MAX_SLAVES) {
		message msg = message(0, ERROR);
		com_send_msg_to_slave(msg, 0);
		if(debug)
			Serial.println("Error: Maximum number of slaves already reached");
		return false;
	}

	// Generate slave pipe address and store it
	const uint64_t lsb = PIPES[slaves.size()];
	uint64_t full_adr =  com_create_pipe_adr(lsb);
	slaves.push_back(full_adr);


	if(debug) {
		Serial.print("Trying to send slave its new address with LSB: ");
		Serial.println((unsigned char) lsb, HEX);
	}

	// Tell the slave it's new address
	message answer = message(0, PAIRING);
	answer.setValue((unsigned char) lsb);
	if(!com_send_msg_to_slave(msg, 0)) {
		if(debug)
			Serial.println("Error: Could ned send msg with address");
		return false;
	}

	if(debug) {
		Serial.println("Address successfully sent!");
	}

	// Wait for answer on the new pipe address
	radio.openReadingPipe(slaves.size(), full_adr);
	message response = com_read_msg_from_slave(slaves.size(), 1000);
	if(response.getComType() != ACK) { // Remove slave from list when there is no ACK
		slaves.pop_back();
		if(debug)
			Serial.println("Error: Did not receive ACK after sending pairing address");
		return false;
	}

	if(debug) {
		Serial.println("Received ACK, handshake complete.");
	}

	return true;
}

bool com_pair_with_master() {
	if(!com_check_for_role(SLAVE)) // Only allowed when being slave
		return false;

	// Send pairing request to master
	message msg = message(0, PAIRING);
	if(!com_send_msg_to_master(msg)) {
		if(debug)
				Serial.println("Error: Could not send PAIRING msg to master");
		return false;
	}

	if(debug) {
		Serial.println("Sent pairing request to Master");
	}

	// Wait for response from master
	msg = com_read_msg_from_master(2000);
	if(msg.getComType() != PAIRING) {
		if(debug)
			Serial.println("Error: Did not receive PAIRING response from master");
		return false;
	}

	if(debug) {
		Serial.print("Received answer from Master, checking address: ");
		Serial.println(msg.getUInt() & 0xFF, HEX);
	}

	// Check for valid address
	uint64_t lsb = msg.getUInt() & 0xFF;
	bool valid_pipe = false;
	for(uint8_t p = 0; p < 5; p++) {
		if(PIPES[p] == lsb) {
			valid_pipe = true;
			break;
		}
	}
	if(!valid_pipe) {
		if(debug)
			Serial.println("Error: Pipe address received is not valid");
		return false;
	}

	if(debug) {
		Serial.println("Address OK and saved. Trying to communicate with Master on new pipe...");
	}

	// Set pipe address and send success message to master
	com_set_pipe(com_create_pipe_adr(lsb));
	msg = message(0, ACK);
	if(!com_send_msg_to_master(msg)) {
		if(debug)
			Serial.println("Communication successful, handshake complete.");
		return true;
	}

	com_set_pipe(DEFAULT_PIPE);
	if(debug)
		Serial.println("Error: Could not send message on new pipe, resetting to default pipe");
	return false;
}

/**
 * Sets the communication pipe for the slave
 */
void com_set_pipe(uint64_t pipe) {
	if(!com_check_for_role(SLAVE)) // Only allowed when being slave
		return;

	com_pipe = pipe;
	radio.openReadingPipe(1, com_pipe);
	radio.startListening();
}


void com_check_interrupt() {
	interrupt = true;
	Serial.println("Interrupt!");
	// What happened?
	bool tx, fail, rx;
	radio.whatHappened(tx, fail, rx);

	// Have we successfully transmitted?
	if (tx) {
		if (sending) {
			if(debug)
				Serial.println("Send: OK");
		} else {
			if(debug)
				Serial.println("Ack Payload: Sent");
		}
	}

	// Have we failed to transmit?
	if (fail) {
		if (sending) {
			if(debug)
				Serial.println("Send: Failed");
		} else {
			if(debug)
				Serial.println("Ack Payload: Failed");
		}
	}

	// Transmitter can power down for now, because
	// the transmission is done.
	//if ((tx || fail) && sending)
	//	radio.powerDown();

	// Did we receive a message?
	if (rx) {
		// If we're the sender, we've received an ack payload
		if (sending) {
			if(debug)
				Serial.println("Received ACK");
		// If we're the receiver, we've received a message
		} else {
			if(debug)
				Serial.println("Message received");
			received_msg = true;
		}
	}

	if(!tx && !rx && !fail) {
		if(debug)
			Serial.println("Interrupt without flags being set...");
	}
}

bool com_available() {
	return radio.available();
}

bool com_check_for_role(uint8_t check_role) {
	if(role == check_role)
		return true;

	if(debug)
		Serial.println("Error: used function, which is not allowed with that role");

	return false;
}

void com_activate_debug() {
	debug = true;
	Serial.println("Activated communication debug mode");
}

void com_deactivate_debug() {
	debug = false;
}

